// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#pragma warning(disable:4201) // glm: nonstandard extension used: nameless struct/union

//#include "modules/map/hdmap/adapter/odr_parser/glm/glm.hpp"
#include "modules/map/hdmap/adapter/odr_parser/roadengine_api.h"

#include <algorithm>
#include <limits>

namespace apollo {
/**
 * Math Utilities.
 */
namespace FWorldMath
{
	constexpr double _E = 2.71828182845904523536;
	constexpr double _LOG2E = 1.44269504088896340736;
	constexpr double _LOG10E = 0.434294481903251827651;
	constexpr double _LN2 = 0.693147180559945309417;
	constexpr double _LN10 = 2.30258509299404568402;
	constexpr double _PI = 3.14159265358979323846;
	constexpr double _PI_2 = 1.57079632679489661923;
	constexpr double _PI_4 = 0.785398163397448309616;
	constexpr double _PI_1_PI = 0.318309886183790671538;
	constexpr double _PI_2_PI = 0.636619772367581343076;
	constexpr double _PI_2_SQRTPI = 1.12837916709551257390;
	constexpr double _SQRT2 = 1.41421356237309504880;
	constexpr double _SQRT1_2 = 0.707106781186547524401;

	// WGS-84 geodetic constants
	namespace wgs
	{
		constexpr double a = 6378137.0;			// WGS-84 Earth semimajor axis (m)
		constexpr double b = 6356752.314245;	// Derived Earth semiminor axis (m)
		constexpr double f = (a - b) / a;		// Ellipsoid Flatness
		constexpr double f_inv = 1.0 / f;		// Inverse flattening

		constexpr double a_sq = a * a;
		constexpr double b_sq = b * b;
		constexpr double e_sq = f * (2 - f);    // Square of Eccentricity
	}

	inline double DegreesToRadians(double degrees)
	{
		return _PI / 180.0 * degrees;
	}

	inline double RadiansToDegrees(double radians)
	{
		return 180.0 / _PI * radians;
	}

	// Reference: https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates
	// Converts WGS-84 Geodetic point (lat, lon, h) to the 
	// Earth-Centered Earth-Fixed (ECEF) coordinates (x, y, z).
	inline glm::dvec3 GeodeticToEcef(double lat, double lon, double h)
	{
		// Convert to radians in notation consistent with the paper:
		double lambda = DegreesToRadians(lat);
		double phi = DegreesToRadians(lon);
		double s = std::sin(lambda);
		double N = wgs::a / std::sqrt(1 - wgs::e_sq * s * s);

		double sin_lambda = std::sin(lambda);
		double cos_lambda = std::cos(lambda);
		double cos_phi = std::cos(phi);
		double sin_phi = std::sin(phi);

		double x = (h + N) * cos_lambda * cos_phi;
		double y = (h + N) * cos_lambda * sin_phi;
		double z = (h + (1 - wgs::e_sq) * N) * sin_lambda;

		return glm::dvec3(x, y, z);
	}

	inline glm::dvec3 GeodeticToEcef(const glm::dvec3& latlonh)
	{
		return GeodeticToEcef(latlonh.x, latlonh.y, latlonh.z);
	}

	//
	// NEED TO CONFIRM. THIS IS DIFFERENT FROM REFERENCE.
	//
	// Reference: https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#The_application_of_Ferrari's_solution
	// Reference: http://wiki-1-1930356585.us-east-1.elb.amazonaws.com/wiki/index.php/Geodetic_system#Step_2:_Convert_ECEF_to_WGS-84
	// Converts the Earth-Centered Earth-Fixed (ECEF) coordinates (x, y, z) to 
	// (WGS-84) Geodetic point (lat, lon, h).
	inline glm::dvec3 EcefToGeodetic(double x, double y, double z)
	{
		double eps = wgs::e_sq / (1.0 - wgs::e_sq);
		double p = std::sqrt(x * x + y * y);
		double q = std::atan2((z * wgs::a), (p * wgs::b));
		double sin_q = std::sin(q);
		double cos_q = std::cos(q);
		double sin_q_3 = sin_q * sin_q * sin_q;
		double cos_q_3 = cos_q * cos_q * cos_q;
		double phi = std::atan2((z + eps * wgs::b * sin_q_3), (p - wgs::e_sq * wgs::a * cos_q_3));
		double lambda = std::atan2(y, x);
		double v = wgs::a / std::sqrt(1.0 - wgs::e_sq * std::sin(phi) * std::sin(phi));

		double h = (p / std::cos(phi)) - v;
		double lat = RadiansToDegrees(phi);
		double lon = RadiansToDegrees(lambda);

		return glm::dvec3(lat, lon, h);
	}

	inline glm::dvec3 EcefToGeodetic(const glm::dvec3& xyz)
	{
		return EcefToGeodetic(xyz.x, xyz.y, xyz.z);
	}

	// Reference: https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU
	// Converts the Earth-Centered Earth-Fixed (ECEF) coordinates (x, y, z) to 
	// East-North-Up coordinates in a Local Tangent Plane that is centered at the 
	// (WGS-84) Geodetic point (lat0, lon0, h0).
	inline glm::dvec3 EcefToEnu(double x, double y, double z, double lat0, double lon0, double h0)
	{
		// Convert to radians in notation consistent with the paper:
		double lambda = DegreesToRadians(lat0);
		double phi = DegreesToRadians(lon0);
		double s = std::sin(lambda);
		double N = wgs::a / std::sqrt(1 - wgs::e_sq * s * s);

		double sin_lambda = std::sin(lambda);
		double cos_lambda = std::cos(lambda);
		double cos_phi = std::cos(phi);
		double sin_phi = std::sin(phi);

		double x0 = (h0 + N) * cos_lambda * cos_phi;
		double y0 = (h0 + N) * cos_lambda * sin_phi;
		double z0 = (h0 + (1 - wgs::e_sq) * N) * sin_lambda;

		double xd, yd, zd;
		xd = x - x0;
		yd = y - y0;
		zd = z - z0;

		// This is the matrix multiplication
		double xEast = -sin_phi * xd + cos_phi * yd;
		double yNorth = -cos_phi * sin_lambda * xd - sin_lambda * sin_phi * yd + cos_lambda * zd;
		double zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd;

		return glm::dvec3(xEast, yNorth, zUp);
	}

	inline glm::dvec3 EcefToEnu(const glm::dvec3& xyz, const glm::dvec3& latlonh0)
	{
		return EcefToEnu(xyz.x, xyz.y, xyz.z, latlonh0.x, latlonh0.y, latlonh0.z);
	}

	// Reference: https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ENU_to_ECEF
	// Inverse of EcefToEnu. Converts East-North-Up coordinates (xEast, yNorth, zUp) in a
	// Local Tangent Plane that is centered at the (WGS-84) Geodetic point (lat0, lon0, h0)
	// to the Earth-Centered Earth-Fixed (ECEF) coordinates (x, y, z).
	inline glm::dvec3 EnuToEcef(double xEast, double yNorth, double zUp, double lat0, double lon0, double h0)
	{
		// Convert to radians in notation consistent with the paper:
		double lambda = DegreesToRadians(lat0);
		double phi = DegreesToRadians(lon0);
		double s = std::sin(lambda);
		double N = wgs::a / std::sqrt(1 - wgs::e_sq * s * s);

		double sin_lambda = std::sin(lambda);
		double cos_lambda = std::cos(lambda);
		double cos_phi = std::cos(phi);
		double sin_phi = std::sin(phi);

		double x0 = (h0 + N) * cos_lambda * cos_phi;
		double y0 = (h0 + N) * cos_lambda * sin_phi;
		double z0 = (h0 + (1 - wgs::e_sq) * N) * sin_lambda;

		double xd = -sin_phi * xEast - cos_phi * sin_lambda * yNorth + cos_lambda * cos_phi * zUp;
		double yd = cos_phi * xEast - sin_lambda * sin_phi * yNorth + cos_lambda * sin_phi * zUp;
		double zd = cos_lambda * yNorth + sin_lambda * zUp;

		double x = xd + x0;
		double y = yd + y0;
		double z = zd + z0;

		return glm::dvec3(x, y, z);
	}

	inline glm::dvec3 EnuToEcef(const glm::dvec3& xyzENU, const glm::dvec3& latlonh0)
	{
		return EnuToEcef(xyzENU.x, xyzENU.y, xyzENU.z, latlonh0.x, latlonh0.y, latlonh0.z);
	}

	// Converts the geodetic WGS-84 coordinated (lat, lon, h) to 
	// East-North-Up coordinates in a Local Tangent Plane that is centered at the 
	// (WGS-84) Geodetic point (lat0, lon0, h0).
	inline glm::dvec3 GeodeticToEnu(double lat, double lon, double h, double lat0, double lon0, double h0)
	{
		return EcefToEnu(GeodeticToEcef(lat, lon, h), glm::dvec3(lat0, lon0, h0));
	}

	inline glm::dvec3 GeodeticToEnu(const glm::dvec3& latlonh, const glm::dvec3& latlonh0)
	{
		return GeodeticToEnu(latlonh.x, latlonh.y, latlonh.z, latlonh0.x, latlonh0.y, latlonh0.z);
	}

	// Reference: https://github.com/natedomin/polyfit
	//----------------------------------------------------
	//
	// METHOD:  polyfit
	//
	// INPUTS:  dependentValues[0..(countOfElements-1)]
	//          independentValues[0...(countOfElements-1)]
	//          countOfElements
	//          order - Order of the polynomial fitting
	//
	// OUTPUTS: coefficients[0..order] - indexed by term
	//               (the (coef*x^3) is coefficients[3])
	//
	//----------------------------------------------------
	inline int Polyfit(const double* const dependentValues,
						const double* const independentValues,
						unsigned int        countOfElements,
						unsigned int        order,
						double*             coefficients)
	{
		// Declarations...
		// ----------------------------------
		enum {maxOrder = 5};
		
		double B[maxOrder+1] = {0.0f};
		double P[((maxOrder+1) * 2)+1] = {0.0f};
		double A[(maxOrder + 1)*2*(maxOrder + 1)] = {0.0f};

		double x, y, powx;

		unsigned int ii, jj, kk;

		// Verify initial conditions....
		// ----------------------------------

		// This method requires that the countOfElements > 
		// (order+1) 
		if (countOfElements <= order)
			return -1;

		// This method has imposed an arbitrary bound of
		// order <= maxOrder.  Increase maxOrder if necessary.
		if (order > maxOrder)
			return -1;

		// Begin Code...
		// ----------------------------------

		// Identify the column vector
		for (ii = 0; ii < countOfElements; ii++)
		{
			x    = dependentValues[ii];
			y    = independentValues[ii];
			powx = 1;

			for (jj = 0; jj < (order + 1); jj++)
			{
				B[jj] = B[jj] + (y * powx);
				powx  = powx * x;
			}
		}

		// Initialize the PowX array
		P[0] = countOfElements;

		// Compute the sum of the Powers of X
		for (ii = 0; ii < countOfElements; ii++)
		{
			x    = dependentValues[ii];
			powx = dependentValues[ii];

			for (jj = 1; jj < ((2 * (order + 1)) + 1); jj++)
			{
				P[jj] = P[jj] + powx;
				powx  = powx * x;
			}
		}

		// Initialize the reduction matrix
		//
		for (ii = 0; ii < (order + 1); ii++)
		{
			for (jj = 0; jj < (order + 1); jj++)
			{
				A[(ii * (2 * (order + 1))) + jj] = P[ii+jj];
			}

			A[(ii*(2 * (order + 1))) + (ii + (order + 1))] = 1;
		}

		// Move the Identity matrix portion of the redux matrix
		// to the left side (find the inverse of the left side
		// of the redux matrix
		for (ii = 0; ii < (order + 1); ii++)
		{
			x = A[(ii * (2 * (order + 1))) + ii];
			if (x != 0)
			{
				for (kk = 0; kk < (2 * (order + 1)); kk++)
				{
					A[(ii * (2 * (order + 1))) + kk] = 
						A[(ii * (2 * (order + 1))) + kk] / x;
				}

				for (jj = 0; jj < (order + 1); jj++)
				{
					if ((jj - ii) != 0)
					{
						y = A[(jj * (2 * (order + 1))) + ii];
						for (kk = 0; kk < (2 * (order + 1)); kk++)
						{
							A[(jj * (2 * (order + 1))) + kk] = 
								A[(jj * (2 * (order + 1))) + kk] -
								y * A[(ii * (2 * (order + 1))) + kk];
						}
					}
				}
			}
			else
			{
				// Cannot work with singular matrices
				return -1;
			}
		}

		// Calculate and Identify the coefficients
		for (ii = 0; ii < (order + 1); ii++)
		{
			for (jj = 0; jj < (order + 1); jj++)
			{
				x = 0;
				for (kk = 0; kk < (order + 1); kk++)
				{
					x = x + (A[(ii * (2 * (order + 1))) + (kk + (order + 1))] *
						B[kk]);
				}
				coefficients[ii] = x;
			}
		}

		return 0;
	}

	inline glm::dvec3 HermiteInterp(const glm::dvec3& P0, const glm::dvec3& M0, const glm::dvec3& P1, const glm::dvec3& M1, double A)
	{
		double A2 = A  * A;
		double A3 = A2 * A;

		return (((2.0 * A3) - (3.0 * A2) + 1.0) * P0) + ((A3 - (2.0 * A2) + A) * M0) + ((A3 - A2) * M1) + (((-2.0 * A3) + (3.0 * A2)) * P1);
	}

	inline glm::dvec3 HermiteDerivative(const glm::dvec3& P0, const glm::dvec3& M0, const glm::dvec3& P1, const glm::dvec3& M1, double A)
	{
		glm::dvec3 a = 6.0*P0 + 3.0*M0 + 3.0*M1 - 6.0*P1;
		glm::dvec3 b = -6.0*P0 - 4.0*M0 - 2.0*M1 + 6.0*P1;
		glm::dvec3 c = M0;

		double A2 = A * A;

		return (a * A2) + (b * A) + c;
	}
}
/*
namespace glm
{
	template <typename T, precision P = defaultp>
	struct tbox3
	{
		// -- Implementation detail --

		typedef T value_type;
		typedef tbox3<T, P> type;
		typedef tvec3<T, P> vec_type;

		// -- Data --

		vec_type min, max;

		// -- Implicit basic constructors --

		GLM_FUNC_DECL GLM_CONSTEXPR_CTOR tbox3()
			: min(std::numeric_limits<T>::max())
			, max(std::numeric_limits<T>::min())
		{}

		// -- Explicit basic constructors --

		GLM_FUNC_DECL GLM_CONSTEXPR_CTOR tbox3(vec_type const & vmin, vec_type const & vmax)
			: min(vmin)
			, max(vmax)
		{}

		// -- Methods --

		GLM_FUNC_DECL vec_type center() const
		{
			return (min + max) * T(0.5);
		}

		// -- Binary arithmetic operators --

		GLM_FUNC_DECL tbox3<T, P> & operator=(tbox3<T, P> const & b) GLM_DEFAULT;

		template <typename U>
		GLM_FUNC_DECL tbox3<T, P> & operator|=(tvec3<U, P> const & v)
		{
			min = glm::min(min, v);
			max = glm::max(max, v);
			return *this;
		}

		template <typename U>
		GLM_FUNC_DECL tbox3<T, P> & operator|=(tbox3<U, P> const & b)
		{
			min = glm::min(min, b.min);
			max = glm::max(max, b.max);
			return *this;
		}
	};

	// -- Binary operators --

	template <typename T, precision P>
	GLM_FUNC_DECL tbox3<T, P> operator|(tbox3<T, P> const & b, tvec3<T, P> const & v)
	{
		tbox3<T, P> r = b;
		r |= v;
		return r;
	}

	template <typename T, precision P>
	GLM_FUNC_DECL tbox3<T, P> operator|(tbox3<T, P> const & b1, tbox3<T, P> const & b2)
	{
		tbox3<T, P> r = b1;
		r |= b2;
		return r;
	}

	typedef tbox3<double, highp>	highp_dbox3;
	typedef highp_dbox3				dbox3;
}
*/
}